/*
 * File:   mcMatrix.h
 *
 * @author Thales Luis Rodrigues Sabino
 * @date   2009
 */

#ifndef _MCMATRIX_H
#define	_MCMATRIX_H

#include <math.h>
#include <assert.h>
#include <stdio.h>
#include <iostream>
#include <string>

#include "mcVector.h"
#include "mcQuaternion.h"

namespace mc {

    template <typename T> class vec2;
    template <typename T> class vec3;
    template <typename T> class vec4;

    template <typename T> class matrix2;
    template <typename T> class matrix3;
    template <typename T> class matrix4;


    /**
     * @brief Classe template para representação de matrizes 2x2
     */
    template<typename T>
    class matrix2 {

        /// Atributos
        public:
            union {
                    struct {
                        T _00, _01;
                        T _10, _11;
                    };

                    T _array[4];     // array access
            };


        /// Métodos
        public:  

            /**
             * @brief Construtor padrão, constroi uma matriz 2x2 com o valor passado
             *
             * @param Valor a ser setado em todas as posições
             */
            matrix2( const T &t ) {
                this->_00 = t; this->_01 = t;
                this->_10 = t; this->_11 = t;
            }
          


            /**
             * @brief Construtor padrão, constroi uma matriz identidade
             *
             * @param Valor a ser setado em todas as posições
             */
            matrix2() {
                this->make_identity();
            }
         


            /**
             * @brief Construtor explicito, constroi uma matriz 2x2 com os valores passados
             *
             * @param __00 - Posição 00
             * @param __01 - Posição 01
             * @param __11 - Posição 11
             * @param __12 - Posição 12
             */
            matrix2(
                const T __00, const T __01,
                const T __10, const T __11
            ) : _00(__00), _01(__01),
                _10(__10), _11(__11)
            {}
 
            

            /**
             * @brief Construtor por cópia de vetor, constroi uma matriz 2x2 com o vetor passado
             *
             * @param m - Matriz a ser copiada
             */
            matrix2( const T *m ) {
                this->set_data(m);
            }
         


            /**
             * @brief Construtor por cópia de matriz.
             *
             * @param m - Matriz a ser copiada
             */
            matrix2( const matrix2<T> &m ) {
                this->_00 = m._00; this->_01 = m._01;
                this->_10 = m._10; this->_11 = m._11;
            }
        


            /**
             * @brief Construtor por cópia de matriz. Retira uma matriz 2x2 de uma 3x3
             *
             * @param m - Matriz a ser copiada
             */
            matrix2( const matrix3<T> &m ) {
                this->_00 = m._00; this->_01 = m._01;
                this->_10 = m._10; this->_11 = m._11;
            }
        


            /**
             * @brief Construtor por cópia de matriz. Retira uma matriz 2x2 de uma 4x4
             *
             * @param m - Matriz a ser copiada
             */
            matrix2( const matrix4<T> &m ) {
                this->_00 = m._00; this->_01 = m._01;
                this->_10 = m._10; this->_11 = m._11;
            }
     

            /**
             * @brief Retorna o número de linhas da matriz
             */
            int get_row_count() { return 2; }

            /**
             * @brief Retorna o número de colunas da matriz
             */
            int get_col_count() { return 2; }

            /**
             * @brief Retorna o número de elementos desta matriz
             */
            int get_size() { return 4; }


            /**
             * @brief Acessa um elemento da matriz
             *
             * @param i - Acesso a linha
             * @param j - Acesso a coluna
             */
            T get_element( int i, int j ) {
                assert( i >= 0 && i < this->get_row_count() && j >= 0 && j < this->get_col_count() );
                return this->_array[this->get_col_count() * j + i];
            }
       

            /**
             * @brief Seta um elemento da matriz 2x2
             *
             * @param i - Linha
             * @param j - Coluna
             * @param e - Elemento a ser setado
             */
            void set_element( int i, int j, T e ) {
                assert( i >= 0 && i < this->get_row_count() && j >= 0 && j < this->get_col_count() );
                this->_array[this->get_col_count() * i + j] = e;
            }
      


            /**
             * @brief Cria uma matriz identidade 2x2
             */
            void make_identity() {
                this->_00 = (T) 1; this->_01 = (T) 0;
                this->_10 = (T) 0; this->_11 = (T) 1;
            }
     

            /**
             * @brief Operador de soma de matrizes 2x2
             *
             * @param m1
             * @param m2
             */
            matrix2<T>& operator + ( const matrix2<T>& m ) {
                matrix2<T> *mr = new matrix2<T>( (T) 0 );
                mr->_00 = this->_00 + mr._00;
                mr->_01 = this->_01 + mr._01;
                mr->_10 = this->_10 + mr._10;
                mr->_11 = this->_11 + mr._11;
                return *mr;
            }
        

            /**
             * @brief Operador de subtração de matrizes 2x2
             *
             * @param m1
             * @param m2
             */
            matrix2<T>& operator - ( const matrix2<T>& m ) {
                matrix2<T> *mr = new matrix2<T>( (T) 0 );
                mr->_00 = this->_00 - m._00;
                mr->_01 = this->_01 - m._01;
                mr->_10 = this->_10 - m._10;
                mr->_11 = this->_11 - m._11;
                return *mr;
            }
       

            /**
             * @brief Operador de multiplicação de matrizes 2x2
             *
             * @param m1
             * @param m2
             */
            matrix2<T>& operator * ( const matrix2<T>& m ) {
                matrix2<T> *mr = new matrix2<T>( (T) 0 );
                mr->_00 = this->_00 * m._00 + this->_01 * m._10;
                mr->_01 = this->_00 * m._01 + this->_01 * m._11;
                mr->_10 = this->_10 * m._00 + this->_11 * m._10;
                mr->_11 = this->_10 * m._01 + this->_11 * m._11;
                return *mr;
            }
  

            /**
             * @brief Operador de soma de matrizes 2x2
             *
             * @param m1
             * @param m2
             */
            matrix2<T>& operator += ( const matrix2<T>& m ) {
                this->_00 += m._00;
                this->_01 += m._01;
                this->_10 += m._10;
                this->_11 += m._11;
                return *this;
            }
      


            /**
             * @brief Operador de subtração de matrizes 2x2
             *
             * @param m1
             * @param m2
             */
            matrix2<T>& operator -= ( const matrix2<T>& m ) {
                this->_00 -= m._00;
                this->_01 -= m._01;
                this->_10 -= m._10;
                this->_01 -= m._01;
                return *this;
            }
    


            /**
             * @brief Operador de multiplicação de matrizes 2x2
             *
             * @param m1
             * @param m2
             */
            matrix2<T>& operator *= ( const matrix2<T>& m ) {
                matrix2<T> *mr = new matrix2<T>( (T) 0 );
                mr->_00 = this->_00 * m._00 + this->_01 * m._10;
                mr->_01 = this->_00 * m._01 + this->_01 * m._11;
                mr->_10 = this->_10 * m._00 + this->_11 * m._10;
                mr->_01 = this->_10 * m._01 + this->_11 * m._11;
                return *mr;
            }



            /**
             * @brief Transpõe a matriz 2x2
             */
            void transpose() {
                matrix2<T> m(this->get_data());
                /// Transpoẽ os dois elementos que serão modificados
                this->_01 = m._10;
                this->_10 = m._01;
            }
      


            /**
             * @brief Transpõe a matriz 2x2
             */
            void transpose( matrix2<T> &m ) {
                /// Transpoẽ os dois elementos que serão modificados
                this->_01 = m._10;
                this->_10 = m._01;
            }
       


            /**
             * @brief Retorna o determinante da matriz 2x2
             *
             * @return O determinante da matriz
             */
            T determinant() {
                return (this->_00 * this->_11 - this->_01 * this->_10);
            }
        


            /**
             * @brief Retorna o vetor com os dados da matriz 2x2
             *
             * @return Os dados da matriz
             */
            T* get_data() {
                return this->_array;
            }
        


            /**
             * @brief Seta o vetor com os dados da matriz 2x2
             */
            void set_data( const T *array ) {
                for( register unsigned int i = 0; i < this->get_size(); i++) this->_array[i] = array[i];
            }


            /**
             * @brief Seta o vetor com os dados da matriz 2x2
             */
            void set_data( const matrix2<T> &m ) {
                this->_00 = m._00;
                this->_01 = m._01;
                this->_10 = m._10;
                this->_11 = m._11;
            }
      


            /**
             * @brief Calcula a inversa desta matriz
             *
             * @return true se a matriz tiver inversa e falso caso contrário
             */
            bool inverse() {
                matrix2<T> m(this->get_data());
                /// Inverte a diagonal principal
                this->_00 = -m._11;
                this->_11 = -m._00;

                T det = this->determinant();

                if( det <= (T) 0 ) return false;

                this->_00 /= det;
                this->_01 /= det;
                this->_10 /= det;
                this->_11 /= det;

                return true;
            }


            /**
             * @brief Calcula a inversa de uma matriz
             *
             * @return true se a matriz tiver inversa e falso caso contra
             */
            bool inverse( const matrix2<T> &m ) {
                this->set_data(m);
                return this->inverse();
            }


            /**
             * @brief Compara duas matrizes para ver se são iguais
             *
             * @param m1 - Matriz
             * @param m2 - Matriz
             */
            friend bool operator == ( const matrix2<T> &m1, const matrix2<T> &m2 ) {
                for( register unsigned int i = 0; i < m1.get_size(); i++ )
                    if( m1._array[i] != m2._array[i] ) return false;

                return true;
            }


            /**
             * @brief Operador para impressão
             */
            friend std::ostream& operator << ( std::ostream &out, const matrix2<T> &m ) {
                char *pOut = new char[256];

                sprintf(
                    pOut,
                    "| %7.4f %7.4f |\n| %7.4f %7.4f |",
                    m._00, m._01,
                    m._10, m._11
                );

                out << pOut;
                delete [] pOut;
                return out;
            }

    };


    /**
     * @brief Classe template para representação de matrizes 3x3
     */
    template<typename T>
    class matrix3 {

        /// Atributos
        public:
            union {
                    struct {
                        T _00, _01, _02;
                        T _10, _11, _12;
                        T _20, _21, _22;
                    };

                    T _array[9];     // array access
            };


        /// Métodos
        public:


            /**
             * @brief Construtor padrão, constroi uma matriz 3x3 com o valor passado
             *
             * @param Valor a ser setado em todas as posições
             */
            matrix3( const T &t = T() ) {
                this->_00 = t; this->_01 = t; this->_02 = t;
                this->_10 = t; this->_11 = t; this->_12 = t;
                this->_20 = t; this->_21 = t; this->_22 = t;
            }
       


            /**
             * @brief Construtor padrão, constroi uma matriz identidade
             *
             * @param Valor a ser setado em todas as posições
             */
            matrix3() {
                this->make_identity();
            }
       


            /**
             * @brief Construtor explicito, constroi uma matriz 3x3 com os valores passados
             *
             * @param __00 - Posição 00
             * @param __01 - Posição 01
             * @param __11 - Posição 11
             * @param __12 - Posição 12
             */
            matrix3(
                const T __00, const T __01, const T __02,
                const T __10, const T __11, const T __12,
                const T __20, const T __21, const T __22
            ) : _00(__00), _01(__01), _02(__02),
                _10(__10), _11(__11), _12(__12),
                _20(__20), _21(__21), _22(__22)
            {}
    


            /**
             * @brief Construtor por cópia de vetor, constroi uma matriz 3x3 com o vetor passado
             *
             * @param m - Matriz a ser copiada
             */
            matrix3( const T *m ) {
                this->set_data(m);
            }
          


            /**
             * @brief Construtor por cópia de matriz. Constrói a partir de uma matriz 2x2
             *
             * @param m - Matriz a ser copiada
             */
            matrix3( const matrix2<T> &m )  {
                this->_00 = m._00; this->_01 = m._01; this->_02 = (T) 0;
                this->_10 = m._10; this->_11 = m._11; this->_12 = (T) 0;
                this->_20 = (T) 0; this->_21 = (T) 0; this->_22 = (T) 0;
            }
       

            /**
             * @brief Construtor por cópia de matriz.
             *
             * @param m - Matriz a ser copiada
             */
            matrix3( const matrix3<T> &m ) {
                this->_00 = m._00; this->_01 = m._01; this->_02 = m._02;
                this->_10 = m._10; this->_11 = m._11; this->_12 = m._12;
                this->_20 = m._20; this->_21 = m._21; this->_22 = m._22;
            }
   

            /**
             * @brief Construtor por cópia de matriz. Constrói a partir de uma matriz 4x4
             *
             * @param m - Matriz a ser copiada
             */
            matrix3( const matrix4<T> &m ) {
                this->_00 = m._00; this->_01 = m._01; this->_02 = m._02;
                this->_10 = m._10; this->_11 = m._11; this->_12 = m._12;
                this->_20 = m._20; this->_21 = m._21; this->_22 = m._22;
            }



            /**
             * @brief Retorna o número de linhas da matriz
             */
            int get_row_count() { return 3; }

            /**
             * @brief Retorna o número de colunas da matriz
             */
            int get_col_count() { return 3; }

            /**
             * @brief Retorna o número de elementos da matriz
             */
            int get_size() { return 9; }


            /**
             * @brief Acessa um elemento da matriz
             *
             * @param i - Acesso a linha
             * @param j - Acesso a coluna
             */
            T get_element( int i, int j ) {
                assert( i >= 0 && i < this->get_row_count() && j >= 0 && j < this->get_col_count() );
                return this->_array[this->get_col_count() * j + i];
            }
   

            /**
             * @brief Seta um elemento da matriz 3x3
             *
             * @param i - Linha
             * @param j - Coluna
             * @param e - Elemento a ser setado
             */
            void set_element( int i, int j, T e ) {
                assert( i >= 0 && i < this->get_row_count() && j >= 0 && j < this->get_col_count() );
                this->_array[this->get_col_count() * j + i] = e;
            }
       


            /**
             * @brief Seta o vetor com os dados da matriz 2x2
             *
             * @return Os dados da matriz
             */
            void set_data( const T *array ) {
                for( register unsigned int i = 0; i < this->get_size(); i++) this->_array[i] = array[i];
            }

            void set_data( const matrix3<T> &m ) {
                this->set_data(m._array);
            }


            /**
             * @brief Cria uma matriz identidade 3x3
             */
            void make_identity() {
                this->_00 = (T) 1; this->_01 = (T) 0; this->_02 = (T) 0;
                this->_10 = (T) 0; this->_11 = (T) 1; this->_12 = (T) 0;
                this->_20 = (T) 0; this->_21 = (T) 0; this->_22 = (T) 1;
            }
      

            /**
             * @brief Operador de soma de matrizes 2x2
             *
             * @param m1
             * @param m2
             */
            matrix3<T>& operator + ( const matrix3<T> &m ) {
                matrix3<T> *mr = new matrix3<T>( (T) 0 );
                mr->_00 = this->_00 + m._00;
                mr->_01 = this->_01 + m._01;
                mr->_02 = this->_02 + m._02;
                mr->_10 = this->_01 + m._02;
                mr->_11 = this->_11 + m._11;
                mr->_12 = this->_12 + m._12;
                mr->_20 = this->_20 + m._20;
                mr->_21 = this->_21 + m._21;
                mr->_22 = this->_22 + m._22;
                return *mr;
            }
        


            /**
             * @brief Operador de subtração de matrizes 3x3
             *
             * @param m1
             * @param m2
             */
            matrix3<T>& operator - ( const matrix3<T> &m ) {
                matrix3<T> *mr = new matrix3<T>( (T) 0 );
                mr->_00 = this->_00 - m._00;
                mr->_01 = this->_01 - m._01;
                mr->_02 = this->_02 - m._02;
                mr->_10 = this->_01 - m._02;
                mr->_11 = this->_11 - m._11;
                mr->_12 = this->_12 - m._12;
                mr->_20 = this->_20 - m._20;
                mr->_21 = this->_21 - m._21;
                mr->_22 = this->_22 - m._22;
                return *mr;
            }
      

            /**
             * @brief Operador de multiplicação de matrizes 3x3
             *
             * @param m1
             * @param m2
             */
            matrix3<T>& operator * ( const matrix3<T> &m ) {
                matrix3<T> *mr = new matrix3<T>( (T) 0 );
                mr->_00 = this->_00 * m->_00 + this->_01 * m->_10 + this->_02 * m->_20;
                mr->_01 = this->_00 * m->_01 + this->_01 * m->_11 + this->_02 * m->_21;
                mr->_02 = this->_00 * m->_02 + this->_01 * m->_12 + this->_02 * m->_22;
                mr->_10 = this->_10 * m->_00 + this->_11 * m->_10 + this->_12 * m->_20;
                mr->_11 = this->_10 * m->_01 + this->_11 * m->_11 + this->_12 * m->_21;
                mr->_12 = this->_10 * m->_02 + this->_11 * m->_12 + this->_12 * m->_22;
                mr->_20 = this->_20 * m->_00 + this->_21 * m->_10 + this->_22 * m->_20;
                mr->_21 = this->_20 * m->_01 + this->_21 * m->_11 + this->_22 * m->_21;
                mr->_22 = this->_20 * m->_02 + this->_21 * m->_12 + this->_22 * m->_22;
                return *mr;
            }


            /**
             * @brief Operador de soma de matrizes 3x3
             *
             * @param m1
             * @param m2
             */
            matrix3<T>& operator += ( const matrix3<T> &m ) {
                this->_00 += m._00;
                this->_01 += m._01;
                this->_02 += m._02;
                this->_01 += m._02;
                this->_11 += m._11;
                this->_12 += m._12;
                this->_20 += m._20;
                this->_21 += m._21;
                this->_22 += m._22;
                return *this;
            }
 


            /**
             * @brief Operador de subtração de matrizes 3x3
             *
             * @param m1
             * @param m2
             */
            matrix3<T>& operator -= ( const matrix3<T> &m ) {
                this->_00 -= m._00;
                this->_01 -= m._01;
                this->_02 -= m._02;
                this->_01 -= m._02;
                this->_11 -= m._11;
                this->_12 -= m._12;
                this->_20 -= m._20;
                this->_21 -= m._21;
                this->_22 -= m._22;
                return *this;
            }
   


            /**
             * @brief Operador de multiplicação de matrizes 3x3
             *
             * @param m1
             * @param m2
             */
            matrix3<T>& operator *= ( const matrix3<T> &m ) {
                matrix3<T> *mr = new matrix3<T>( (T) 0 );
                mr->_00 = this->_00 * m._00 + this->_01 * m._10 + this->_02 * m._20;
                mr->_01 = this->_00 * m._01 + this->_01 * m._11 + this->_02 * m._21;
                mr->_02 = this->_00 * m._02 + this->_01 * m._12 + this->_02 * m._22;
                mr->_10 = this->_10 * m._00 + this->_11 * m._10 + this->_12 * m._20;
                mr->_11 = this->_10 * m._01 + this->_11 * m._11 + this->_12 * m._21;
                mr->_12 = this->_10 * m._02 + this->_11 * m._12 + this->_12 * m._22;
                mr->_20 = this->_20 * m._00 + this->_21 * m._10 + this->_22 * m._20;
                mr->_21 = this->_20 * m._01 + this->_21 * m._11 + this->_22 * m._21;
                mr->_22 = this->_20 * m._02 + this->_21 * m._12 + this->_22 * m._22;
                return *mr;
            }



            /**
             * @brief Transpõe a matriz 3x3
             */
            void transpose() {
                matrix3<T> m(this->get_data());
                this->_01 = m._10;
                this->_02 = m._20;
                this->_10 = m._01;
                this->_12 = m._21;
                this->_20 = m._02;
                this->_21 = m._12;
            }
  


            /**
             * @brief Transpõe a matriz 3x3
             */
            void transpose( matrix3<T> &m )  {
                /// Transpoẽ os dois elementos que serão modificados
                this->_01 = m._10;
                this->_02 = m._20;
                this->_10 = m._01;
                this->_12 = m._21;
                this->_20 = m._02;
                this->_21 = m._12;
            }



            /**
             * @brief Retorna o determinante da matriz 3x3
             *
             * @return O determinante da matriz
             */
            T determinant() {
                return ((_00*_11*_22 + _10*_21*_02 + _01*_12*_20) - (_02*_11*_20 + _12*_21*_00 + _22*_01*_20));
            }
      


            /**
             * @brief Retorna o vetor com os dados da matriz 3x3
             *
             * @return Os dados da matriz
             */
            T* get_data() {
                return this->_array;
            }
     

            /**
             * @brief Calcula a inversa desta matriz
             *
             * @return true se a matriz tiver inversa e falso caso contrário
             */
            bool inverse() {
                matrix3<T> m(this->get_data());

                T det = this->determinant();

                if( det <= (T) 0 ) return false;

                T det_i = ((T)1) / det;

                this->_00 = ( m._22*m._11 - m._21*_12 ) / det_i;
                this->_01 = ( m._22*m._01 - m._21*_01 ) / det_i;
                this->_10 = ( m._12*m._01 - m._11*_01 ) / det_i;
                this->_11 = ( m._22*m._00 - m._20*_02 ) / det_i;
                this->_12 = ( m._12*m._00 - m._10*_01 ) / det_i;
                this->_20 = ( m._21*m._10 - m._20*_11 ) / det_i;
                this->_21 = ( m._21*m._00 - m._20*_01 ) / det_i;
                this->_22 = ( m._11*m._00 - m._10*_01 ) / det_i;

                return true;
            }


            /**
             * @brief Calcula a inversa de uma matriz
             *
             * @return true se a matriz tiver inversa e falso caso contra
             */
            bool inverse( const matrix3<T> &m ) {
                this->set_data(m);
                return this->inverse();
            }

            

            /**
             * @brief Compara duas matrizes para ver se são iguais
             *
             * @param m1 - Matriz
             * @param m2 - Matriz
             */
            friend bool operator == ( const matrix3<T> &m1, const matrix3<T> &m2 ) {
                for( register unsigned int i = 0; i < m1.get_size(); i++ )
                    if( m1._array[i] != m2._array[i] ) return false;

                return true;
            }


            /**
             * @brief Operador para impressão
             */
            friend std::ostream& operator << ( std::ostream &out, const matrix3<T> &m ) {
                char *pOut = new char[256];

                sprintf(
                    pOut,
                    "| %7.4f %7.4f %7.4f |\n| %7.4f %7.4f %7.4f |\n| %7.4f %7.4f %7.4f |",
                    m._00, m._01, m._02,
                    m._10, m._11, m._12,
                    m._20, m._21, m._22
                );

                out << pOut;
                delete [] pOut;
                return out;
            }

    };


    /**
     * @brief Classe template para representação de matrizes 4x4
     */
    template<typename T>
    class matrix4 {

        /// Atributos
        public:
            union {
                    struct {
                        T _00, _01, _02, _03;
                        T _10, _11, _12, _13;
                        T _20, _21, _22, _23;
                        T _30, _31, _32, _33;
                    };

                    T _array[16];     // array access
            };


        /// Métodos
        public:

            /**
             * @brief Construtor padrão, constroi uma matriz 4x4 com o valor passado
             *
             * @param Valor a ser setado em todas as posições
             */
            matrix4( const T t ) {
                this->_00 = t; this->_01 = t; this->_02 = t; this->_03 = t;
                this->_10 = t; this->_11 = t; this->_12 = t; this->_13 = t;
                this->_20 = t; this->_21 = t; this->_22 = t; this->_23 = t;
                this->_30 = t; this->_31 = t; this->_32 = t; this->_33 = t;
            }



            /**
             * @brief Construtor padrão, constroi uma matriz identidade
             *
             * @param Valor a ser setado em todas as posições
             */
            matrix4() {
                this->make_identity();
            }
       


            /**
             * @brief Construtor explicito, constroi uma matriz 3x3 com os valores passados
             *
             * @param __00 - Posição 00
             * @param __01 - Posição 01
             * @param __11 - Posição 11
             * @param __12 - Posição 12
             */
            matrix4(
                const T __00, const T __01, const T __02, const T __03,
                const T __10, const T __11, const T __12, const T __13,
                const T __20, const T __21, const T __22, const T __23,
                const T __30, const T __31, const T __32, const T __33
            ) : _00(__00), _01(__01), _02(__02), _03(__03),
                _10(__10), _11(__11), _12(__12), _13(__13),
                _20(__20), _21(__21), _22(__22), _23(__23),
                _30(__30), _31(__31), _32(__32), _33(__33)
            {}



            /**
             * @brief Construtor por cópia de vetor, constroi uma matriz 3x3 com o vetor passado
             *
             * @param m - Matriz a ser copiada
             */
            matrix4( const T *m ) {
                this->set_data(m);
            }



            /**
             * @brief Construtor por cópia de matriz. Constrói a partir de uma matriz 2x2
             *
             * @param m - Matriz a ser copiada
             */
            matrix4( const matrix2<T> &m ) {
                this->_00 = m._00; this->_01 = m._01; this->_02 = (T) 0; this->_03 = (T) 0;
                this->_10 = m._10; this->_11 = m._11; this->_12 = (T) 0; this->_13 = (T) 0;
                this->_20 = (T) 0; this->_21 = (T) 0; this->_22 = (T) 0; this->_23 = (T) 0;
                this->_30 = (T) 0; this->_31 = (T) 0; this->_32 = (T) 0; this->_33 = (T) 0;
            }
    

            /**
             * @brief Construtor por cópia de matriz.
             *
             * @param m - Matriz a ser copiada
             */
            matrix4( const matrix3<T> &m ) {
                this->_00 = m._00; this->_01 = m._01; this->_02 = m._02; this->_03 = (T) 0;
                this->_10 = m._10; this->_11 = m._11; this->_12 = m._12; this->_13 = (T) 0;
                this->_20 = m._20; this->_21 = m._21; this->_22 = m._22; this->_23 = (T) 0;
                this->_30 = (T) 0; this->_31 = (T) 0; this->_32 = (T) 0; this->_33 = (T) 0;
            }
 

            /**
             * @brief Construtor por cópia de matriz. Constrói a partir de uma matriz 4x4
             *
             * @param m - Matriz a ser copiada
             */
            matrix4( const matrix4<T> &m ) {
                this->_00 = m._00; this->_01 = m._01; this->_02 = m._02; this->_03 = m._03;
                this->_10 = m._10; this->_11 = m._11; this->_12 = m._12; this->_13 = m._13;
                this->_20 = m._20; this->_21 = m._21; this->_22 = m._22; this->_23 = m._23;
                this->_30 = m._30; this->_31 = m._31; this->_32 = m._32; this->_33 = m._33;
            }
 


            /**
             * @brief Retorna o número de linhas da matriz
             *
             * @return Número de linhas da matriz
             */
            int get_row_count() { return 4; }

            /**
             * @brief Retorna o número de colunas da matriz
             *
             * @return Número de colunas da matriz
             */
            int get_col_count() { return 4; }

            /**
             * @brief Retorna o número de elementos da matriz
             *
             * @return O número de elementos da matriz
             */
            int get_size() { return 16; }


            /**
             * @brief Acessa um elemento da matriz
             *
             * @param i - Acesso a linha
             * @param j - Acesso a coluna
             */
            T get_element( int i, int j ) {
                assert( i >= 0 && i < this->get_row_count() && j >= 0 && j < this->get_col_count() );
                return this->_array[this->get_col_count() * j + i];
            }
         

            /**
             * @brief Seta um elemento da matriz 3x3
             *
             * @param i - Linha
             * @param j - Coluna
             * @param e - Elemento a ser setado
             */
            void set_element( int i, int j, T e ) {
                assert( i >= 0 && i < this->get_row_count() && j >= 0 && j < this->get_col_count() );
                this->_array[this->get_col_count() * j + i] = e;
            }
     

            /**
             * @brief Seta o vetor com os dados da matriz 4x4
             *
             * @return Os dados da matriz
             */
            void set_data( const T *array ) {
                for( register unsigned int i = 0; i < this->get_size(); i++) this->_array[i] = array[i];
            }

            void set_data( const matrix4<T> &m ) {
                this->set_data(m._array);
            }
       



            /**
             * @brief Cria uma matriz identidade 4x4
             */
            void make_identity() {
                this->_00 = (T) 1; this->_01 = (T) 0; this->_02 = (T) 0; this->_03 = (T) 0;
                this->_10 = (T) 0; this->_11 = (T) 1; this->_12 = (T) 0; this->_13 = (T) 0;
                this->_20 = (T) 0; this->_21 = (T) 0; this->_22 = (T) 1; this->_23 = (T) 0;
                this->_30 = (T) 0; this->_31 = (T) 0; this->_32 = (T) 0; this->_33 = (T) 1;
            }
      

            /**
             * @brief Operador de soma de matrizes 4x4
             *
             * @param m1
             * @param m2
             */
            matrix4<T>& operator + ( const matrix4<T> &m2 ) {
                matrix4<T> *mr = new matrix4<T>( (T) 0 );
                mr->_00 = this->_00 + m2._00;
                mr->_01 = this->_01 + m2._01;
                mr->_02 = this->_02 + m2._02;
                mr->_03 = this->_03 + m2._03;
                mr->_10 = this->_01 + m2._02;
                mr->_11 = this->_11 + m2._11;
                mr->_12 = this->_12 + m2._12;
                mr->_13 = this->_13 + m2._13;
                mr->_20 = this->_20 + m2._20;
                mr->_21 = this->_21 + m2._21;
                mr->_22 = this->_22 + m2._22;
                mr->_23 = this->_23 + m2._23;
                mr->_30 = this->_30 + m2._30;
                mr->_31 = this->_31 + m2._31;
                mr->_32 = this->_32 + m2._32;
                mr->_33 = this->_33 + m2._33;
                return *mr;
            }
  


            /**
             * @brief Operador de subtração de matrizes 4x4
             *
             * @param m1
             * @param m2
             */
            matrix4<T>& operator - ( const matrix4<T> &m ) {
                matrix4<T> *mr = new matrix4<T>( (T) 0 );
                mr->_00 = this->_00 - m._00;
                mr->_01 = this->_01 - m._01;
                mr->_02 = this->_02 - m._02;
                mr->_03 = this->_03 - m._03;
                mr->_10 = this->_01 - m._02;
                mr->_11 = this->_11 - m._11;
                mr->_12 = this->_12 - m._12;
                mr->_13 = this->_13 - m._13;
                mr->_20 = this->_20 - m._20;
                mr->_21 = this->_21 - m._21;
                mr->_22 = this->_22 - m._22;
                mr->_23 = this->_23 - m._23;
                mr->_30 = this->_30 - m._30;
                mr->_31 = this->_31 - m._31;
                mr->_32 = this->_32 - m._32;
                mr->_33 = this->_33 - m._33;
                return *mr;
            }
  

            /**
             * @brief Operador de multiplicação de matrizes 4x4
             *
             * @param m1
             * @param m2
             */
            matrix4<T>& operator * ( const matrix4<T> &m ) {
                matrix4<T> *mr = new matrix4<T>( (T) 0 );
                mr->_00 = this->_00 * m._00 + this->_01 * m._10 + this->_02 * m._20 + this->_03 * m._30;
                mr->_01 = this->_00 * m._01 + this->_01 * m._11 + this->_02 * m._21 + this->_03 * m._31;
                mr->_02 = this->_00 * m._02 + this->_01 * m._12 + this->_02 * m._22 + this->_03 * m._32;
                mr->_03 = this->_00 * m._03 + this->_01 * m._13 + this->_02 * m._23 + this->_03 * m._33;

                mr->_10 = this->_10 * m._00 + this->_11 * m._10 + this->_12 * m._20 + this->_13 * m._30;
                mr->_11 = this->_10 * m._01 + this->_11 * m._11 + this->_12 * m._21 + this->_13 * m._31;
                mr->_12 = this->_10 * m._02 + this->_11 * m._12 + this->_12 * m._22 + this->_13 * m._32;
                mr->_13 = this->_10 * m._03 + this->_11 * m._13 + this->_12 * m._23 + this->_13 * m._33;

                mr->_20 = this->_20 * m._00 + this->_21 * m._10 + this->_22 * m._20 + this->_23 * m._30;
                mr->_21 = this->_20 * m._01 + this->_21 * m._11 + this->_22 * m._21 + this->_23 * m._31;
                mr->_22 = this->_20 * m._02 + this->_21 * m._12 + this->_22 * m._22 + this->_23 * m._32;
                mr->_23 = this->_20 * m._03 + this->_21 * m._13 + this->_22 * m._23 + this->_23 * m._33;

                mr->_30 = this->_30 * m._00 + this->_31 * m._10 + this->_32 * m._20 + this->_33 * m._30;
                mr->_31 = this->_30 * m._01 + this->_31 * m._11 + this->_32 * m._21 + this->_33 * m._31;
                mr->_32 = this->_30 * m._02 + this->_31 * m._12 + this->_32 * m._22 + this->_33 * m._32;
                mr->_33 = this->_30 * m._03 + this->_31 * m._13 + this->_32 * m._23 + this->_33 * m._33;
                return *mr;
            }
   

            /**
             * @brief Operador de soma de matrizes 4x4
             *
             * @param m1
             * @param m2
             */
            matrix4<T>& operator += ( const matrix4<T> &m ) {
                this->_00 += m._00;
                this->_01 += m._01;
                this->_02 += m._02;
                this->_03 += m._03;
                this->_10 += m._02;
                this->_11 += m._11;
                this->_12 += m._12;
                this->_13 += m._13;
                this->_20 += m._20;
                this->_21 += m._21;
                this->_22 += m._22;
                this->_23 += m._23;
                this->_30 += m._30;
                this->_31 += m._31;
                this->_32 += m._32;
                this->_33 += m._33;
                return *this;
            }



            /**
             * @brief Operador de subtração de matrizes 4x4
             *
             * @param m1
             * @param m2
             */
            matrix4<T>& operator -= ( const matrix4<T> &m ) {
                this->_00 -= m._00;
                this->_01 -= m._01;
                this->_02 -= m._02;
                this->_03 -= m._03;
                this->_10 -= m._02;
                this->_11 -= m._11;
                this->_12 -= m._12;
                this->_13 -= m._13;
                this->_20 -= m._20;
                this->_21 -= m._21;
                this->_22 -= m._22;
                this->_23 -= m._23;
                this->_30 -= m._30;
                this->_31 -= m._31;
                this->_32 -= m._32;
                this->_33 -= m._33;
                return *this;
            }



            /**
             * @brief Operador de multiplicação de matrizes 4x4
             *
             * @param m1
             * @param m2
             */
            matrix4<T>& operator *= ( const matrix4<T> &m ) {
                matrix4<T> *mr = new matrix4<T>( (T) 0 );
                mr->_00 = m._00 * this->_00 + m._01 * this->_10 + m._02 * this->_20 + m._03 * this->_30;
                mr->_01 = m._00 * this->_01 + m._01 * this->_11 + m._02 * this->_21 + m._03 * this->_31;
                mr->_02 = m._00 * this->_02 + m._01 * this->_12 + m._02 * this->_22 + m._03 * this->_32;
                mr->_03 = m._00 * this->_03 + m._01 * this->_13 + m._02 * this->_23 + m._03 * this->_33;

                mr->_10 = m._10 * this->_00 + m._11 * this->_10 + m._12 * this->_20 + m._13 * this->_30;
                mr->_11 = m._10 * this->_01 + m._11 * this->_11 + m._12 * this->_21 + m._13 * this->_31;
                mr->_12 = m._10 * this->_02 + m._11 * this->_12 + m._12 * this->_22 + m._13 * this->_32;
                mr->_13 = m._10 * this->_03 + m._11 * this->_13 + m._12 * this->_23 + m._13 * this->_33;

                mr->_20 = m._20 * this->_00 + m._21 * this->_10 + m._22 * this->_20 + m._23 * this->_30;
                mr->_21 = m._20 * this->_01 + m._21 * this->_11 + m._22 * this->_21 + m._23 * this->_31;
                mr->_22 = m._20 * this->_02 + m._21 * this->_12 + m._22 * this->_22 + m._23 * this->_32;
                mr->_23 = m._20 * this->_03 + m._21 * this->_13 + m._22 * this->_23 + m._23 * this->_33;

                mr->_30 = m._30 * this->_00 + m._31 * this->_10 + m._32 * this->_20 + m._33 * this->_30;
                mr->_31 = m._30 * this->_01 + m._31 * this->_11 + m._32 * this->_21 + m._33 * this->_31;
                mr->_32 = m._30 * this->_02 + m._31 * this->_12 + m._32 * this->_22 + m._33 * this->_32;
                mr->_33 = m._30 * this->_03 + m._31 * this->_13 + m._32 * this->_23 + m._33 * this->_33;
                return *mr;
            }



            /**
             * @brief Transpõe a matriz 4x4
             */
            void transpose() {
                matrix4<T> m(this->get_data());
                this->_01 = m._10;
                this->_02 = m._20;
                this->_03 = m._30;
                this->_10 = m._01;
                this->_12 = m._21;
                this->_13 = m._31;
                this->_20 = m._02;
                this->_21 = m._12;
                this->_23 = m._32;
                this->_30 = m._03;
                this->_31 = m._13;
                this->_32 = m._23;
            }
  


            /**
             * @brief Transpõe a matriz 4x4
             */
            void transpose( matrix4<T> &m ) {
                this->set_data(m);
                this->transpose();
            }



            /**
             * @brief Retorna o determinante da matriz 4x4
             *
             * @return O determinante da matriz
             */
            T determinant() {
                matrix3<T> m1(
                    _11, _12, _13,
                    _21, _22, _23,
                    _31, _32, _33
                );

                matrix3<T> m2(
                    _01, _02, _03,
                    _21, _22, _23,
                    _31, _32, _33
                );

                matrix3<T> m3(
                    _01, _02, _03,
                    _11, _12, _13,
                    _31, _32, _33
                );

                matrix3<T> m4(
                    _01, _02, _03,
                    _11, _12, _13,
                    _21, _22, _23
                );

                T det1 = m1.determinant();
                T det2 = m2.determinant();
                T det3 = m3.determinant();
                T det4 = m4.determinant();

                return _00 * det1 - _10 * det2 + _20 * det3 - _30 * det4;
            }



            /**
             * @brief Retorna o vetor com os dados da matriz 3x3
             *
             * @return Os dados da matriz
             */
            T* get_data() {
                return this->_array;
            }
   

            /**
             * @brief Calcula a inversa desta matriz
             *
             * @return true se a matriz tiver inversa e falso caso contrário
             */
            bool inverse() {
                matrix4<T> m(this->get_data());

                T det = this->determinant();

                if( det <= (T) 0 ) return false;

                /// Calcula o inverso do determinante
                T det_i = ((T)1) / det;

                /// Monta as matriz de cofatores dos elementos
                matrix3<T> a00(
                    _11, _12, _13,
                    _21, _22, _23,
                    _31, _32, _33
                );

                matrix3<T> a01(
                    _10, _12, _13,
                    _20, _22, _23,
                    _30, _32, _33
                );

                matrix3<T> a02(
                    _10, _11, _13,
                    _20, _21, _23,
                    _30, _31, _33
                );

                matrix3<T> a03(
                    _10, _11, _12,
                    _20, _21, _22,
                    _30, _31, _32
                );


                matrix3<T> a10(
                    _01, _02, _03,
                    _21, _22, _23,
                    _31, _32, _33
                );

                matrix3<T> a11(
                    _00, _02, _03,
                    _20, _22, _23,
                    _30, _32, _33
                );

                matrix3<T> a12(
                    _00, _01, _03,
                    _20, _21, _23,
                    _30, _31, _33
                );

                matrix3<T> a13(
                    _00, _01, _02,
                    _20, _21, _22,
                    _30, _31, _32
                );

                matrix3<T> a20(
                    _01, _02, _03,
                    _11, _12, _13,
                    _31, _32, _33
                );

                matrix3<T> a21(
                    _00, _02, _03,
                    _10, _12, _13,
                    _30, _32, _33
                );

                matrix3<T> a22(
                    _00, _01, _03,
                    _10, _11, _13,
                    _30, _31, _33
                );

                matrix3<T> a23(
                    _00, _01, _02,
                    _10, _11, _12,
                    _30, _31, _32
                );

                matrix3<T> a30(
                    _01, _02, _03,
                    _11, _12, _13,
                    _21, _22, _23
                );

                matrix3<T> a31(
                    _00, _02, _03,
                    _10, _12, _13,
                    _20, _22, _23
                );

                matrix3<T> a32(
                    _00, _01, _03,
                    _10, _11, _13,
                    _20, _21, _23
                );

                matrix3<T> a33(
                    _00, _01, _02,
                    _10, _11, _12,
                    _20, _21, _22
                );

                /// Calcula o determinante das matrizes adjuntas
                T det00 = a00.determinant();
                T det01 = a01.determinant();
                T det02 = a02.determinant();
                T det03 = a03.determinant();

                T det10 = a10.determinant();
                T det11 = a11.determinant();
                T det12 = a12.determinant();
                T det13 = a13.determinant();

                T det20 = a20.determinant();
                T det21 = a21.determinant();
                T det22 = a22.determinant();
                T det23 = a23.determinant();

                T det30 = a30.determinant();
                T det31 = a31.determinant();
                T det32 = a32.determinant();
                T det33 = a33.determinant();

                /// Finalmente calcula o determinante final
                this->_00 =  det00 * det_i;
                this->_01 = -det01 * det_i;
                this->_02 =  det02 * det_i;
                this->_03 = -det03 * det_i;

                this->_10 =  det10 * det_i;
                this->_11 = -det11 * det_i;
                this->_12 =  det12 * det_i;
                this->_13 = -det13 * det_i;

                this->_20 =  det20 * det_i;
                this->_21 = -det21 * det_i;
                this->_22 =  det22 * det_i;
                this->_23 = -det23 * det_i;

                this->_30 =  det30 * det_i;
                this->_31 = -det31 * det_i;
                this->_32 =  det32 * det_i;
                this->_33 = -det33 * det_i;

                return true;
            }


            /**
             * @brief Calcula a inversa de uma matriz
             *
             * @return true se a matriz tiver inversa e falso caso contra
             */
            bool inverse( matrix4<T> &m ) {
                this->set_data(m);
                return this->inverse();
            }


            /**
             * @brief Compara duas matrizes para ver se são iguais
             *
             * @param m1 - Matriz
             * @param m2 - Matriz
             */
            friend bool operator == ( const matrix4<T> &m1, const matrix4<T> &m2 ) {
                for( register unsigned int i = 0; i < m1.get_size(); i++ )
                    if( m1._array[i] != m2._array[i] ) return false;

                return true;
            }

            /**
             * @brief Operador para impressão
             */
            friend std::ostream& operator << ( std::ostream &out, const matrix4<T> &m ) {
                char *pOut = new char[256];

                sprintf(
                    pOut,
                    "| %7.4f %7.4f %7.4f %7.4f |\n| %7.4f %7.4f %7.4f %7.4f |\n| %7.4f %7.4f %7.4f %7.4f |\n| %7.4f %7.4f %7.4f %7.4f |",
                    m._00, m._01, m._02, m._03,
                    m._10, m._11, m._12, m._13,
                    m._20, m._21, m._22, m._23,
                    m._30, m._31, m._32, m._33
                );

                out << pOut;
                delete [] pOut;
                return out;
            }

    };

};



#endif	/* _MCMATRIX_H */

